%% Kim Filter

function [PS_tt, XI_tt, XI_ttl, x_tt, P_tt, lnL] = Kim_Filter_MA_sg(theta,y,NS,Nx)

T = numel(y);

% declare spae for output %%%
XI_tt  = 189*ones(NS,T);       XI_ttl  = 189*ones(NS,T+1); 
PS_tt  = 189*ones(NS,NS,T);
x_tt   = 189*ones(Nx,NS,T);    
P_tt   = 189*ones(Nx,Nx,NS,T); 

lnL = 0;                   % log-likelihood
R = theta.R;
A = theta.A; G = theta.G; Q = theta.Q; 
x00  = theta.x00;  P00  = theta.P00;
Pmat  = theta.Pmat;
muvec = theta.muvec;

% auxiliar variables
xtt_ax   = 189*ones(Nx,NS,NS);
Ptt_ax   = 189*ones(Nx,Nx,NS,NS);
x_tltl   = 189*ones(Nx,NS);
P_tltl   = 189*ones(Nx,Nx,NS);
ff       = 189*ones(NS,NS);
PSttl_ax = 189*ones(NS,NS);

% initialize filter
x_tltl(:,1)   = x00; x_tltl(:,2)   = x00;
P_tltl(:,:,1) = P00; P_tltl(:,:,2) = P00;
XI_tltl     = theta.XI_00;
XI_ttl(:,1) = (Pmat')*theta.XI_00; 

for tt = 1: T
    
    for is    = 1:NS
    for is_lp = 1:NS
        %%% Kalman filter step
        Fij = [theta.sgvec(is), -theta.sgvec(is_lp)];
        % forecast 
        xttl_ax = A*x_tltl(:,is_lp);
        Pttl_ax = A*P_tltl(:,:,is_lp)*A' + G*Q*G';
        eta_ttl = y(tt) - muvec(is) - Fij*xttl_ax;
        H       = Fij*Pttl_ax*Fij' + R;
        KK      = Pttl_ax*(Fij')*(1/H);        
        % update        
        xtt_ax(:,is_lp,is)   = xttl_ax + KK*eta_ttl;
        Ptt_ax(:,:,is_lp,is) = (eye(Nx) - KK*Fij)*Pttl_ax;       
        % evaluate pdf        
        dd           = eta_ttl/sqrt(H);                  
        ff(is_lp,is) = normpdf(dd,0,1)/sqrt(H);        
        %%% Hamilton filter step
        PSttl_ax(is_lp,is) = Pmat(is_lp,is)*XI_tltl(is_lp);        
        PS_tt(is_lp,is,tt) = ff(is_lp,is)*PSttl_ax(is_lp,is);
    end
    end    
    
    fax           = PS_tt(:,:,tt);
    fsum          = sum(fax(:));
    PS_tt(:,:,tt) = PS_tt(:,:,tt)/fsum;    
    
    % update inference and iteration objects
    XI_tt(:,tt) = 0; x_tt(:,:,tt) = 0;
    for is    = 1:NS
        for is_lp = 1:NS
            XI_tt(is,tt)  = XI_tt(is,tt)  + PS_tt(is_lp,is,tt);        
            x_tt(:,is,tt) = x_tt(:,is,tt) + xtt_ax(:,is_lp,is)*PS_tt(is_lp,is,tt);        
        end
        x_tt(:,is,tt) = x_tt(:,is,tt)/XI_tt(is,tt);
    end
    P_tt(:,:,:,tt) = 0;
    for is = 1:NS
        for is_lp = 1:NS
            ax              = Ptt_ax(:,:,is_lp,is) + (x_tt(:,is,tt) - xtt_ax(:,is_lp,is))*((x_tt(:,is,tt) - xtt_ax(:,is_lp,is))');
            P_tt(:,:,is,tt) = P_tt(:,:,is,tt) + PS_tt(is_lp,is,tt)*ax;
        end
        P_tt(:,:,is,tt) = P_tt(:,:,is,tt)/XI_tt(is,tt);
    end
       
    XI_ttl(:,tt+1) = (Pmat')*XI_tt(:,tt);    
    XI_tltl = XI_tt(:,tt);
    x_tltl  = x_tt(:,:,tt);
    P_tltl  = P_tt(:,:,:,tt);
       
    lnL = lnL + log(fsum);   
   
end

return;